#if 0

#include "cvaux.h"
#include "cxmisc.h"
#include "highgui.h"
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include "cvblob.h"
#include <fstream>
#include <math.h>

#include <boost/filesystem.hpp>   // includes all needed Boost.Filesystem declarations
#include <iostream>               // for std::cout

//add dx dy and with every frame when blob is invisible change it!

using namespace boost;
using namespace std;
using namespace cvb;

class OBlob{
public:
	unsigned int x;
	unsigned int y;
	unsigned int S;
	unsigned int w;
	unsigned int h;
	unsigned int r;
	unsigned int g;
	unsigned int b;
};

class OBject:public OBlob{
public:
	unsigned int id;	//if id=0, object doesnt exist
	unsigned int a;		//appearance number if a>5 - track it
	unsigned int e;		//edited, found, interrpretted
	unsigned int ls;	//last seen, if ls>50 remove
	int px;				//previous x
	int py;				//previous y
};


class Track{
	public:
		Track();
		//Track(unsigned int maxXfr, unsigned int maxYfr);
		~Track(){};
		void Update(OBlob oblob, int nfr);
		void Render(IplImage *fr);
		void SetSizes(unsigned int x, unsigned int y){maxX=x; maxY=y;};
	private:
		CvFont font;
		unsigned int maxX;
		unsigned int maxY;
		unsigned int suspicious[20];
		unsigned int nrSusp;
		OBject list[20];
		int currentFrame;
		int findObjectByXY(int x, int y, int &id);
		int findObjectByS(unsigned int w, unsigned int h, unsigned int S, int &id);
		int findObjectByRGB(unsigned int r, unsigned int g, unsigned int b, int &id);
		void updateObject(OBlob ob, int id);
		void addObject(OBlob ob);
		void removeOld();
};


CvScalar getColorInfo(unsigned int minx, unsigned int maxx, unsigned int miny, unsigned int maxy, IplImage *frame){
	int tab[3][255];
	CvScalar sc;
	int maxVal=0;
	unsigned int x=0,y=0;
	int step       = frame->widthStep/sizeof(uchar);
	int channels   = frame->nChannels;
	int surface=(maxx-minx)*(maxy-miny);
	uchar* data    = (uchar *)frame->imageData;
	int pos=0;

	cout << "\n";
	for(int i=0; i<3; ++i){
		for(int ii=0; ii<255; ++ii){
			tab[i][ii]=0;}}
	//cout << "E22\n";
	sc.val[0]=0; sc.val[1]=0; sc.val[2]=0;

	//cout << "E23\n";
	// Get color info for three channels (RGB?)
	for(y=miny; y<maxy; ++y){
		for(x=minx; x<maxx; ++x)
		{
			++tab[2][(int)data[y*step+x*channels]];
			++tab[1][(int)data[y*step+x*channels+1]];
			++tab[0][(int)data[y*step+x*channels+2]];
		}
	}

	//cout << "E24\n";
	// Find max value in adjacent fields (channel 0)
	for(int i=1; i<254; ++i){
		if((tab[0][i-1]+tab[0][i]+tab[0][i+1])>maxVal){
			maxVal=(tab[0][i-1]+tab[0][i]+tab[0][i+1]);
			pos=i;
		}
	}

	//cout << "E25\n";
	// Find max value in adjacent fields (channel 1)
	sc.val[2]=pos;
	pos=0; maxVal=0;
	for(int i=1; i<254; ++i){
		if((tab[1][i-1]+tab[1][i]+tab[1][i+1])>maxVal){
			maxVal=(tab[1][i-1]+tab[1][i]+tab[1][i+1]);
			pos=i;
		}
	}

	//cout << "E26\n";
	// Find max value in adjacent fields (channel 2)
	sc.val[1]=pos;
	pos=0; maxVal=0;
	for(int i=1; i<254; ++i){
		if((tab[2][i-1]+tab[2][i]+tab[2][i+1])>maxVal){
			maxVal=(tab[2][i-1]+tab[2][i]+tab[2][i+1]);
			pos=i;
		}
	}
	//cout << "E27\n";
	sc.val[0]=pos;
	return sc;
}


CvBGCodeBookModel* model = 0;
const int NCHANNELS = 3;
bool ch[NCHANNELS]={true,true,true}; // This sets what channels should be adjusted for background bounds


int main(int argc, char** argv)
{
	//CvTracks tracks;
    IplImage *rawImgOrg = 0;
    IplImage* rawImage = 0;
    IplImage *ImaskCodeBook = 0;
    CvCapture* capture = 0;
	IplConvKernel* morphKernel1 = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT, NULL);
    IplConvKernel* morphKernel2 = cvCreateStructuringElementEx(5, 5, 3, 3, CV_SHAPE_RECT, NULL);
    ofstream myfile;
	int nframes = 0;
	Track tr;
	char c;
	//CvVideoWriter *writer = 0;
    //int nframesToLearnBG = 300;
    model = cvCreateBGCodeBookModel();
    //Set color thresholds to default values
    model->modMin[0] = 3;
    model->modMin[1] = model->modMin[2] = 3;
    model->modMax[0] = 10;
    model->modMax[1] = model->modMax[2] = 10;
    model->cbBounds[0] = model->cbBounds[1] = model->cbBounds[2] = 10;

   // bool pause = false;
   // bool singlestep = false

    if (!boost::filesystem::exists("/root/workspace/DisplayImage/wildlife.wmf"))
    {
    	printf( "File not found\n\n" );
    	return -1;
    }


    //capture = cvCreateFileCapture("/root/workspace/DisplayImage/wildlife.wmf");
    capture = cvCaptureFromCAM(0);

    if( !capture )
    {
        printf( "Can not initialize video capturing\n\n" );
        return -1;
    }

	//MAIN PROCESSING LOOP:
    for(;;)
    {
    	rawImgOrg = cvQueryFrame( capture );

		// declare a destination IplImage object with correct size, depth and channels
		rawImage = cvCreateImage( cvSize( (int)(rawImgOrg->width*0.5), (int)(rawImgOrg->height*0.5) ),
								rawImgOrg->depth, rawImgOrg->nChannels );

		//use cvResize to resize source to a destination image
		cvResize(rawImgOrg, rawImage);

		// save image with a name supplied with a second argument
		//cvSaveImage( argv[2], destination );


        ++nframes;

        if(!rawImage)
			break;

        // First step
        if( nframes == 1 && rawImage )
        {

            ImaskCodeBook = cvCreateImage( cvGetSize(rawImage), IPL_DEPTH_8U, 1 );
			cvSet(ImaskCodeBook,cvScalar(255));
			tr.SetSizes(rawImage->width, rawImage->height);
			int isColor = 1;
			int fps     = 30;
			int frameW  = 384;
			int frameH  = 288;
			//writer=cvCreateVideoWriter("out.avi",-1,
            //               fps,cvSize(frameW,frameH),isColor);

			// Create output windows
            cvNamedWindow( "Raw", CV_WINDOW_AUTOSIZE);
            cvNamedWindow( "ForegroundCodeBook",CV_WINDOW_AUTOSIZE);
            //cvNamedWindow("tracking", CV_WINDOW_AUTOSIZE);
		}

        // Every other step
        if( rawImage )
        {
            double t = (double)cvGetTickCount();
			if((nframes<100)){
				    cvBGCodeBookUpdate( model, rawImage );
					continue;}
			else
				cvBGCodeBookDiff( model, rawImage, ImaskCodeBook );
			if(nframes==100)
				cvBGCodeBookClearStale( model, model->t/2 );
            cvMorphologyEx(ImaskCodeBook, ImaskCodeBook, NULL, morphKernel1, CV_MOP_ERODE, 1);
			cvMorphologyEx(ImaskCodeBook, ImaskCodeBook, NULL, morphKernel2, CV_MOP_DILATE, 1);
			IplImage *labelImg = cvCreateImage(cvGetSize(ImaskCodeBook), IPL_DEPTH_LABEL, 1);
			CvBlobs blobs;
			unsigned int result = cvLabel(ImaskCodeBook, labelImg, blobs);
			//cvFilterByArea(blobs, 100, 1000000);
			//cvRenderBlobs(labelImg, blobs, rawImage, rawImage, CV_BLOB_RENDER_BOUNDING_BOX);
			//if((nframes==633)) {
			//	cvSaveImage("633a.bmp",ImaskCodeBook);
			//	cvSaveImage("633b.bmp",rawImage);
			//	myfile.open("633.txt");
		//	cout << "E1\n";
			for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it){
				OBlob ob1;
				ob1.x=(unsigned int)it->second->centroid.x;
				ob1.y=(unsigned int)it->second->centroid.y;
				ob1.S=it->second->area;
				ob1.w=it->second->maxx-it->second->minx;
				ob1.h=it->second->maxy-it->second->miny;
			//	cout << "E2\n";
				CvScalar scal=getColorInfo(it->second->minx,it->second->maxx,it->second->miny,it->second->maxy,rawImage);
				//cout << "E3\n";
				ob1.r=(unsigned int)scal.val[0];
				ob1.g=(unsigned int)scal.val[1];
				ob1.b=(unsigned int)scal.val[2];
//				cout << "E4\n";
				//cout << "I will add ob1(" <<ob1.x<<","<<ob1.y<<")\n";
				tr.Update(ob1,nframes);
	//			cout << "E5\n";
			}

			//	cout << "E6\n";
			//	myfile.close();
			//}
			/*if((nframes==638)) {
				cvSaveImage("638a.bmp",ImaskCodeBook);
				cvSaveImage("638b.bmp",rawImage);
				myfile.open("638.txt");
				for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it){
					myfile << (int)it->second->centroid.x <<" " << (int)it->second->centroid.y << " " << it->second->area << " ";
					myfile << it->second->maxx-it->second->minx <<" " << it->second->maxy-it->second->miny << " ";
					CvScalar scal=getColorInfo(it->second->minx,it->second->maxx,it->second->miny,it->second->maxy,rawImage);
					myfile << scal.val[0] << " "<< scal.val[1] << " "<< scal.val[2] << "\n";
				}
				myfile.close();
			}*/
			//cvUpdateTracks(blobs, tracks, 200., 5);
			//cvRenderTracks(tracks, rawImage, rawImage, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX);
			t = (double)cvGetTickCount() - t;
			cout << nframes << ". " << t/(cvGetTickFrequency()*1000.) << endl;
			//printf( "%d. %.1f\n\n", nframes, t/(cvGetTickFrequency()*1000.) );
			tr.Render(rawImage);
          //  cout << "E7\n";
			//if((nframes>100)&&(nframes<2150))
			//	cvWriteFrame(writer,ImaskCodeBook);

			cvShowImage( "Raw", rawImage );
            cvShowImage( "ForegroundCodeBook", ImaskCodeBook);
			//cvShowImage("tracking", labelImg);

			cvReleaseImage(&labelImg);
			cvReleaseBlobs(blobs);
		}
		c = cvWaitKey(5);
        if(c == 27 || c == 'q')
            break;
		if(c == 'p')
            cvWaitKey(0);

    }
	//cvReleaseVideoWriter(&writer);
    cvReleaseStructuringElement(&morphKernel1);
	cvReleaseStructuringElement(&morphKernel2);
    cvReleaseCapture( &capture );
    cvDestroyWindow( "Raw" );
    cvDestroyWindow( "ForegroundCodeBook");
    return 0;
}


void Track::Render(IplImage *fr){
	CvScalar kol;
	kol=cvScalar(255,0,0,0);
	CvScalar kol2;
	kol2=cvScalar(0,255,0,0);

	removeOld();
	for(int i=0; i<20; ++i){
		char id[2];
		if((list[i].id!=0)&&(list[i].a>5)){
			if(list[i].ls!=currentFrame) {
				if((currentFrame-list[i].ls)<10){
					sprintf(id,"%d",list[i].id);
					cvCircle(fr,cvPoint(list[i].x,list[i].y),ceil(pow((double)(list[i].S),0.5)),kol2,2);
					cvPutText(fr,id,cvPoint(list[i].x,list[i].y), &font, kol2);
					id[0]='\0'; id[1]='\0';
				}
			}
			else{
				sprintf(id,"%d",list[i].id);
				cvCircle(fr,cvPoint(list[i].x,list[i].y),ceil(pow((double)(list[i].S),0.5)),kol,2);
				cvPutText(fr,id,cvPoint(list[i].x,list[i].y), &font, kol);
				id[0]='\0'; id[1]='\0';
			}
		}
	}
}

Track::Track(){
	double hScale=1.0;
	double vScale=1.0;
	int    lineWidth=2;
	cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale,vScale,0,lineWidth);
	for(int i=0; i<20; ++i){
		list[i].id=0;
	}
}

void Track::Update(OBlob ob, int nfr){
	currentFrame=nfr;
	int nrXY, nrS, nrCOL, nrDX;
	int id=0;
	nrXY=findObjectByXY(ob.x,ob.y,id);
	nrS=findObjectByS(ob.w,ob.h,ob.S,id);
	if(nrS==0) nrXY=0;
	//nrCOL=findObjectByRGB(ob.x,ob.y,id);

	if(nrXY==1)
		updateObject(ob,id);
	if(nrXY==0)
		addObject(ob);


}

int Track::findObjectByXY(int x, int y, int &id){
	int number=0;
	int prevDist=101;
	int disty=0;
	int distx=0;
	nrSusp=0;
	int radius=20;
	for(int i=0; i<20; ++i){
		if(list[i].id!=0){
			distx=list[i].x-x;
			if(distx<0) distx*=-1;
			disty=list[i].y-y;
			if(disty<0) disty*=-1;
			//cout << "Distance=" << distx+disty <<"("<<distx<<", "<<disty<<")" << endl;
			radius=20+floor((double)((10/7)*(currentFrame-list[i].ls)+(60/7)));
			if((distx+disty)<radius){
				if((distx+disty)<prevDist){
					prevDist=distx+disty;
				//suspicious[nrSusp]=i;
				//nrSusp+=1;
					number=1;
					id=i+1;
				}
			}
		}
	}
return number;
}

int Track::findObjectByS(unsigned int w, unsigned int h, unsigned int S, int &id){
	//int number=0;
	//int prevDist=121;
	int disth=0;
	int distw=0;
	int distS=0;
	//for(int i=0; i<nrSusp; ++i){
		//if(list[i].id!=0){
			disth=list[id-1].h-h;
			if(disth<0) disth*=-1;
			distw=list[id-1].w-w;
			if(distw<0) distw*=-1;
			distS=list[id-1].S-S;
			if(distS<0) distS*=-1;
			//cout << "Distance=" << distw+disth+distS <<"("<<distw<<", "<<disth<<", "<<distS<<")" << endl;
			if(((distw+disth)<84)&&(distS<256)){
				//if((distw+disth+distS)<prevDist){
					//prevDist=distw+disth+distS;
					//number=1;
					//id=i+1;
				return 1;
				}
return 0;
}

void Track::updateObject(OBlob ob, int id){
	//cout << "Updating object " <<id-1<< " which has e=" << list[id-1].e << endl;
	if(list[id-1].e>1){
		list[id-1].x=(unsigned int)(list[id-1].x+ob.x)/2;
		list[id-1].y=(unsigned int)(list[id-1].y+ob.y)/2;
		list[id-1].S+=ob.S;
		list[id-1].w=(unsigned int)(0.5*max(ob.w,list[id-1].w)+0.5*min(ob.w,list[id-1].w));
		list[id-1].h=(unsigned int)(0.5*max(ob.h,list[id-1].h)+0.5*min(ob.h,list[id-1].h));
		list[id-1].r=(unsigned int)((ob.r+list[id-1].r)/2);
		list[id-1].g=(unsigned int)((ob.g+list[id-1].g)/2);
		list[id-1].b=(unsigned int)((ob.b+list[id-1].b)/2);
		//cout << "uPdated ob x: "<< ob.x <<", y:" <<ob.y << endl;
	}
	else{
		//list[id-1].px=list[id-1].x;
		//list[id-1].py=list[id-1].y;
		list[id-1].x=ob.x;
		list[id-1].y=ob.y;
		list[id-1].S=ob.S;
		list[id-1].w=ob.w;
		list[id-1].h=ob.h;
		list[id-1].r=ob.r;
		list[id-1].g=ob.g;
		list[id-1].b=ob.b;
		++list[id-1].a;
		list[id-1].ls=currentFrame;
		list[id-1].e=2;
		//cout << "updAted ob x: "<< ob.x <<", y:" <<ob.y << endl;
	}

}

void Track::addObject(OBlob ob){
	int i=0;
	while(list[i].id!=0){
		++i;
		if(i>20) return;
	}
	list[i].id=i+1;
	list[i].x=ob.x;
	list[i].y=ob.y;
	list[i].S=ob.S;
	//cout << "Added ob x: "<< ob.x <<", y:" <<ob.y << endl;
	list[i].w=ob.w;
	list[i].h=ob.h;
	list[i].r=ob.r;
	list[i].g=ob.g;
	list[i].b=ob.b;
	list[i].a=1;
	list[i].ls=currentFrame;
	list[i].e=1;
	//list[i].px=0;
	//list[i].py=0;
}

void Track::removeOld(){
	for(int i=0; i<20; ++i){
		if(list[i].id!=0){
			if(list[i].ls!=currentFrame){
				if(list[i].a<5){
					if((list[i].ls+5)<currentFrame){
						list[i].id=0;
					}
				}
				else{
					if((list[i].ls+50)<currentFrame){
						list[i].id=0;
					}
				}
				if(list[i].x<0){list[i].x=0; list[i].px=0;}
				if(list[i].x>maxX-2){list[i].x=maxX-1; list[i].px=maxX-1;}
				if(list[i].y<0){list[i].y=0; list[i].py=0;}
				if(list[i].y>maxY-2){list[i].y=maxY-1; list[i].py=maxY-1;}
			}
		}
		list[i].e=1;
	}
}
/*
*/

#endif
